#include "motor_qdrive_pm4310.h"

bool pm4310::initialize() {
    initialized = true;
    return true;
}

bool pm4310::enable() {
    if (!initialized) return false;
    TxBuffer[0] = CMD_ENABLE;
    send_can_message();
    enabled = true;
    return true;
}

bool pm4310::disable() {
    if (!initialized) return false;
    if (enabled) enabled = false;
    TxBuffer[0] = CMD_DISABLE;
    return true;
}

bool pm4310::start() {
    if (!initialized || !enabled) return false;
    if (!started) {
        started = true;
    }
    return true;
}

bool pm4310::stop() {
    if (!initialized || !enabled) return false;
    if (started) {}
    return true;
}

void pm4310::update(uint8_t *data) {
    angle = *reinterpret_cast<uint16_t *>(data + 6) * 2 * numbers::pi_v<float> / UINT16_MAX;
    speed = *reinterpret_cast<int16_t *>(data + 4) * 5000.0f / INT16_MAX;
    current = *reinterpret_cast<int16_t *>(data + 2) * 10.0f / INT16_MAX;
}

void pm4310::set_torque(float torque) {
    int16_t current = torque / 10 * INT16_MAX;
    TxBuffer[0] = CMD_MOTION_CTRL;
    TxBuffer[1] = MOTION_MODE_CUR;
    *reinterpret_cast<int16_t *>(TxBuffer + 2) = current;
    send_can_message();
}

void pm4310::set_speed(float speed) {
    int16_t speed_ = speed / 5000 * INT16_MAX;
    TxBuffer[0] = CMD_MOTION_CTRL;
    TxBuffer[1] = MOTION_MODE_SPD;
    *reinterpret_cast<int16_t *>(TxBuffer + 2) = speed_;
    send_can_message();
}

void pm4310::set_angle(float angle) {
    int16_t angle_ = angle / (2 * numbers::pi_v<float>) * UINT16_MAX;
    TxBuffer[0] = CMD_MOTION_CTRL;
    TxBuffer[1] = MOTION_MODE_ANG;
    *reinterpret_cast<int16_t *>(TxBuffer + 2) = angle_;
    send_can_message();
}

void pm4310::set_id(uint8_t id)
{
    TxBuffer[0] = CMD_SET_ID;
    TxBuffer[1] = 0x00;
    TxBuffer[2] = 0x00;
    TxBuffer[3] = 0x00;
    TxBuffer[4] = 0x00;
    TxBuffer[5] = 0x00;
    TxBuffer[6] = 0x00;
    TxBuffer[7] = id;
    send_can_message();
}

void pm4310::send_can_message() const {
    const FDCAN_TxHeaderTypeDef TxHeader = {
        .Identifier = 0x400 + id,
        .IdType = FDCAN_STANDARD_ID,
        .TxFrameType = FDCAN_DATA_FRAME,
        .DataLength = FDCAN_DLC_BYTES_8,
        .ErrorStateIndicator = FDCAN_ESI_ACTIVE,
        .BitRateSwitch = FDCAN_BRS_OFF,
        .FDFormat = FDCAN_CLASSIC_CAN,
        .TxEventFifoControl = FDCAN_NO_TX_EVENTS,
        .MessageMarker = 0
    };
    HAL_FDCAN_AddMessageToTxFifoQ(hfdcan, &TxHeader, TxBuffer);
}
